 #include "car_config.h"     // 头文件

#define RtA 		57.295779f	//弧度->角度
#define AtR    		0.0174533f	//角度->弧度
#define Acc_G 		0.0011963f
#define Gyro_G 		0.0610351f
#define Gyro_Gr		0.0010653f

#define Kp 18.0f
#define Ki 0.008f
#define halfT 0.008f

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
float Angle[3]={0};//最终角度


void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz)
{
	float ax=accx,ay=accy,az=accz;
	float gx=gyrox,gy=gyroy,gz=gyroz;
  	float norm;
  	float vx, vy, vz;
  	float ex, ey, ez;
  	float q0q0 = q0*q0;
  	float q0q1 = q0*q1;
  	float q0q2 = q0*q2;
  	float q1q1 = q1*q1;
  	float q1q3 = q1*q3;
  	float q2q2 = q2*q2;
  	float q2q3 = q2*q3;
  	float q3q3 = q3*q3;
	if(ax*ay*az==0)//此时任意方向角加速度为0
 		return;
 	gx *= Gyro_Gr;
	gy *= Gyro_Gr;
	gz *= Gyro_Gr;

  	norm = sqrt(ax*ax + ay*ay + az*az);
  	ax = ax /norm;
  	ay = ay / norm;
  	az = az / norm;

 	 // estimated direction of gravity and flux (v and w)
  	vx = 2*(q1q3 - q0q2);
  	vy = 2*(q0q1 + q2q3);
  	vz = q0q0 - q1q1 - q2q2 + q3q3 ;

  	// error is sum of cross product between reference direction of fields and direction 		measured by sensors
  	ex = (ay*vz - az*vy) ;
  	ey = (az*vx - ax*vz) ;
  	ez = (ax*vy - ay*vx) ;

  	exInt = exInt + ex * Ki;
  	eyInt = eyInt + ey * Ki;
  	ezInt = ezInt + ez * Ki;

    // adjusted gyroscope measurements
  	gx = gx + Kp*ex + exInt;
  	gy = gy + Kp*ey + eyInt;
  	gz = gz + Kp*ez + ezInt;

  	// integrate quaternion rate and normalise
  	q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  	q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  	q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  	q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

  	// normalise quaternion
  	norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

  	q0 = q0 / norm;
  	q1 = q1 / norm;
  	q2 = q2 / norm;
  	q3 = q3 / norm;

	//Angle[3]  = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; 			// yaw 无意义
  	Angle[0]= asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitch 俯仰角
  	Angle[1]= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA;  	//roll  翻滚角
	//printf("pitch = %f  roll = %f  \n",Angle[0],Angle[1]);
}

